#include <ros/ros.h>
#include <std_msgs/Int8.h>
#include <std_msgs/UInt32.h>
#include <serial/serial.h>
#include <chrono>
#include <thread>
// 函数：获取低八位
int getLowByte(int num)
{
    return num & 0xFF; // 0xFF 是 11111111 (二进制)，表示只保留最低的 8 位
}

// 函数：获取次低八位
int getnext_LowByte(int num)
{
    return (num >> 8) & 0xFF; // 右移 8 位，然后与 0xFF 按位与，保留次低 8 位
}

// 函数：获取次高八位
int getnext_HighByte(int num)
{
    return (num >> 16) & 0xFF; // 右移 8 位，然后与 0xFF 按位与，保留次高 8 位
}

// 函数：获取高八位
int getHighByte(int num)
{
    return (num >> 24) & 0xFF; // 右移 8 位，然后与 0xFF 按位与，保留高 8 位
}

uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
{
    uint8_t crc_u = crc ^ crc_seed;

    for (int i = 0; i < 8; ++i)
    {
        crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
    }

    return crc_u;
}

// CRC8校验
uint8_t crc8(const uint8_t *buf, uint8_t len)
{
    uint8_t crc = 0;

    for (int i = 0; i < len; ++i)
    {
        crc = updateCrc8(buf[i], crc);
    }

    return crc;
}

// 串口连接
serial::Serial ser;

void timerCallback(const ros::TimerEvent &)
{
    ROS_INFO("定时器触发了!");
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "motor_control_node");
    ros::NodeHandle nh;

    // 配置串口
    ser.setPort("/dev/ttyUSB0");
    ser.setBaudrate(115200);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();

    if (!ser.isOpen())
    {
        ROS_ERROR("Failed to open serial port");
        return 1;
    }

    // int speed = 4000;
    // int amplitude = 20;
    // int phase = 6560;
    int publish_frequency = 1; // 1000 Hz (1秒发布1000次)
    int publish_duration = 1;    // 发布时长为10秒
    int phase = 20;
    int amplitude = 20;
    int speed = 4000;
    // int time = 100;
    for (int i = 0; i < 10; i++)
    {
        // int speed = 4000;
        for (int j = 0; j < 5; j++)
        {
            // int amplitude = 0;
            for (int k = 0; k < 5; k++)
            {
                // int phase = 0;
                ros::Rate loop_rate(publish_frequency);  // 控制发布频率
                ros::Time start_time = ros::Time::now(); // 记录开始时间
                ros::Duration elapsed_time = ros::Time::now() - start_time;
                while (ros::ok() && elapsed_time.toSec() <= publish_duration )
                {
                    
                   
                    uint8_t head_1 = 0xAA;
                    uint8_t head_2 = 0X55;
                    uint8_t speed_low = getLowByte(speed);
                    uint8_t speed_next_low = getnext_LowByte(speed);
                    uint8_t speed_next_high = getnext_HighByte(speed);
                    uint8_t speed_high = getHighByte(speed);
                    uint8_t amplitude_low = getLowByte(amplitude);
                    uint8_t amplitude_next_low = getnext_LowByte(amplitude);
                    uint8_t amplitude_next_high = getnext_HighByte(amplitude);
                    uint8_t amplitude_high = getHighByte(amplitude);
                    uint8_t phase_low = getLowByte(phase);
                    uint8_t phase_next_low = getnext_LowByte(phase);
                    uint8_t phase_next_high = getnext_HighByte(phase);
                    uint8_t phase_high = getHighByte(phase);
                    // uint8_t time_low = getLowByte(time);
                    // uint8_t time_next_low = getnext_LowByte(time);
                    // uint8_t time_next_high = getnext_HighByte(time);
                    // uint8_t time_high = getHighByte(time);
                    // ROS_INFO_STREAM(head_1);
                    // ROS_INFO_STREAM(head_2);
                    // int num1 = speed_high << 24 | speed_next_high << 16 | speed_next_low << 8 | speed_low;
                    // int num2 = amplitude_high << 24 | amplitude_next_high << 16 | amplitude_next_low << 8 | amplitude_low;
                    // int num3 = phase_high << 24 | phase_next_high << 16 | phase_next_low << 8 | phase_low;
                    // int num4 = time_high << 24 | time_next_high << 16 | time_next_low << 8 | time_low;
                    // ROS_INFO_STREAM(num1);
                    // ROS_INFO_STREAM(num2);
                    // ROS_INFO_STREAM(num3);
                    // ROS_INFO_STREAM(num4);
                    uint8_t number[15] = {
                        head_1,
                        head_2,
                        speed_low,
                        speed_next_low,
                        speed_next_high,
                        speed_high,
                        amplitude_low,
                        amplitude_next_low,
                        amplitude_next_high,
                        amplitude_high,
                        phase_low,
                        phase_next_low,
                        phase_next_high,
                        phase_high,
                        // time_low,
                        // time_next_low,
                        // time_next_high,
                        // time_high,
                    };
                    uint8_t crcCode = crc8(number, 14);
                    number[15] = crcCode;
                    // 发送速度和时间指令给STM32
                    if (ser.isOpen())
                    {
                        ser.write(number, sizeof(number)); // 发送
                    }

                    loop_rate.sleep(); // 控制发布频率
                    
                }
                phase += 20;
            }
            amplitude += 20;
        }
        speed += 100;
    }

    ros::spin();
    ser.close();
    return 0;
}
